import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, ExecuteProcess
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch.substitutions import Command
from launch_ros.parameter_descriptions import ParameterValue

def generate_launch_description():
    # 获取包目录
    pkg_dir = get_package_share_directory('realsense_gazebo_plugin')
    
    world_path = os.path.join(pkg_dir, 'worlds', 'realsense_test.world')
    # 模型路径参数
    model = LaunchConfiguration('model', default=PathJoinSubstitution([
        pkg_dir, 'urdf', 'test.xacro'
    ]))
    
    # 将XACRO转换为URDF的命令:cite[3]:cite[7]
    xacro_command = ExecuteProcess(
        cmd=['ros2', 'run', 'xacro', 'xacro', model, '-o', '/tmp/robot.urdf'],
        output='screen'
    )
    
    # 生成URDF描述参数
    robot_description_content = ExecuteProcess(
        cmd=['ros2', 'run', 'xacro', 'xacro', model],
        output='screen'
    )
    
    # 在Gazebo中生成URDF模型:cite[10]
    spawn_entity = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=[
            '-entity', 'test_model',
            '-file', '/tmp/robot.urdf',
            '-z', '1.0',
            '-unpause'
        ],
        output='screen'
    )
    
    # 启动Gazebo空世界:cite[10]
    gazebo_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            get_package_share_directory('gazebo_ros'),
            '/launch/gazebo.launch.py'
        ]),
        launch_arguments={
            'world': world_path,
            'paused': 'true',
            'use_sim_time': 'true',
            'gui': 'true',
            'headless': 'false',
            'debug': 'false'
        }.items()
    )
    
    # 关节状态发布器:cite[4]:cite[9]
    joint_state_publisher = Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher',
        output='screen'
    )
    
    # 机器人状态发布器:cite[5]:cite[9]
    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output='screen',
        parameters=[{
            'robot_description': ParameterValue(
                Command(['xacro ', model]), value_type=str
            )
        }]
    )
        
    return LaunchDescription([
        xacro_command,
        gazebo_launch,
        joint_state_publisher,
        robot_state_publisher,
        # spawn_entity
    ])